#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <emmintrin.h>


#define NPARTS 1000
#define NITER 201
#define DIMS 3

int rand( void );
int computePot(void);
void initPositions(void);
void updatePositions(void);

double r[NPARTS][DIMS];
double pot;
double distx, disty, distz, dist;

int main() {
    int i;
    clock_t start, stop;
    
    initPositions();
    updatePositions();
    
    start=clock();
    for( i=0; i<NITER; i++ ) {
        pot = 0.0;
        computePot();
        if (i%10 == 0) printf("%5d: Potential: %10.7f \n", i, pot);
        updatePositions();
    }
    stop=clock();
    printf ("Seconds = %10.9f  \n",(double)(stop-start)/ CLOCKS_PER_SEC);
    getchar();
}


void initPositions() {
    int i, j;
    
    for( i=0; i<DIMS; i++ )
        for( j=0; j<NPARTS; j++ )
            r[j][i] = 0.5 + ( (double) rand() / (double) RAND_MAX );
}


void updatePositions() {
    int i, j;
    
    for( i=0; i<DIMS; i++ )
        for( j=0; j<NPARTS; j++ )
            r[j][i] -= 0.5 + ( (double) rand() / (double) RAND_MAX );
}

inline double pow2(const double x) { return x*x; }

int computePot() {
    int i, j;
    __m128d lcpot;
    
    lcpot=_mm_setzero_pd();
    for( i=0; i<NPARTS; i++ ) {
        __m128d posi_l=*(__m128d*)(&r[i][0]);
        __m128d posi_h=*(__m128d*)(&r[i][2]);
        j=0;
        for(;j+1<i;++j)
        {
            __m128d dm0_l=_mm_sub_pd(*(__m128d*)(&r[j][0]),posi_l);
            __m128d dm0_h=_mm_sub_pd(*(__m128d*)(&r[j][2]),posi_h);
            dm0_l=_mm_mul_pd(dm0_l,dm0_l);
            dm0_h=_mm_mul_pd(dm0_h,dm0_h);
            
            __m128d dm0=_mm_add_pd(dm0_l,dm0_h);
            __m128d dm1;
            dm1=_mm_unpackhi_pd(dm0,dm0);
            dm0=_mm_add_sd(dm0,dm1);
            
            
            dm1=_mm_sqrt_sd(dm0,dm0);
            dm0=_mm_div_sd(dm1,dm0);
            
            lcpot=_mm_add_sd(lcpot,dm0);
        }
    }
    
    __m128d dm1;
    dm1=_mm_unpackhi_pd(lcpot,lcpot);
    lcpot=_mm_add_sd(lcpot,dm1);
    pot+=*(double*)(&lcpot);
    
    return 0;
}
